﻿#include "ServoManager.h"
#include "global.h"
#include "base/Command.h"
#include "base/ServerMonitor.h"
#include "QZXing.h"
bool pipeRunFlag {true};

ServoManager::ServoManager(QObject *parent) : QObject(parent)
{

}

void ServoManager::GraphBuild()
{
    /*存档案
        XY跑至出货口->(拍照->定位->识别)->Z进->夹->Y抬升->Z出(->RB)->XY跑目标位置->Z进->Y下降->松->Z出*/
    pPort2StorgeLine = CGraph::GPipelineFactory::create();

    pP2S_XY_Port_Node = new XYMoveNode(XYMOVETYPE::type_Port,this);
    CGraph::GElementPtr _pP2S_XY_Port_Node = pP2S_XY_Port_Node;
    CStatus status = pPort2StorgeLine->registerGNode(&_pP2S_XY_Port_Node,{},"pP2S_XY_Port_Node",1);

    pP2S_Img_Port_Node = new CameraNode(this);
    CGraph::GElementPtr _pP2S_Img_Port_Node = pP2S_Img_Port_Node;
    status = pPort2StorgeLine->registerGNode(&_pP2S_Img_Port_Node,{_pP2S_XY_Port_Node},"_pP2S_Img_Port_Node",1);

    pP2S_ZIn_Port_Node = new ZMoveNode(ZMOVETYPE::type_In,this);
    CGraph::GElementPtr _pP2S_ZIn_Port_Node = pP2S_ZIn_Port_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_ZIn_Port_Node,{_pP2S_Img_Port_Node},"pP2S_ZIn_Port_Node",1);

    pP2S_Grab_Port_Node = new GrabNode(GRABTYPE::type_Clampe,this);
    CGraph::GElementPtr _pP2S_Grab_Port_Node = pP2S_Grab_Port_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_Grab_Port_Node,{_pP2S_ZIn_Port_Node},"pP2S_Grab_Port_Node",1);

    pP2S_YStep_Port_Node = new YStepNode(YSTEPTYPE::type_UP,this);
    CGraph::GElementPtr _pP2S_YStep_Port_Node = pP2S_YStep_Port_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_YStep_Port_Node,{_pP2S_Grab_Port_Node},"pP2S_YStep_Port_Node",1);

    pP2S_ZOut_Port_Node = new ZMoveNode(ZMOVETYPE::type_Out,this);
    CGraph::GElementPtr _pP2S_ZOut_Port_Node = pP2S_ZOut_Port_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_ZOut_Port_Node,{_pP2S_YStep_Port_Node},"pP2S_ZOut_Port_Node",1);

    pP2S_RB_Node = new RotateNode(ROTATETYPE::type_FaceB,this);
    CGraph::GElementPtr _pP2S_RB_Node = pP2S_RB_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_RB_Node,{_pP2S_ZOut_Port_Node},"pP2S_RB_Node",1);

    pP2S_XY_Storge_Node = new XYMoveNode(XYMOVETYPE::type_Storge,this);
    CGraph::GElementPtr _pP2S_XY_Storge_Node = pP2S_XY_Storge_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_XY_Storge_Node,{_pP2S_RB_Node},"pP2S_XY_Storge_Node",1);

    pP2S_Img_Storge_Node = new CameraNode(this);
    CGraph::GElementPtr _pP2S_Img_Storge_Node = pP2S_Img_Storge_Node;
    status = pPort2StorgeLine->registerGNode(&_pP2S_Img_Storge_Node,{_pP2S_XY_Storge_Node},"_pP2S_Img_Port_Node",1);

    pP2S_ZIn_Storge_Node = new ZMoveNode(ZMOVETYPE::type_In,this);
    CGraph::GElementPtr _pP2S_ZIn_Storge_Node = pP2S_ZIn_Storge_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_ZIn_Storge_Node,{_pP2S_Img_Storge_Node},"pP2S_ZIn_Storge_Node",1);

    pP2S_YStep_Storge_Node = new YStepNode(YSTEPTYPE::type_DOWN,this);
    CGraph::GElementPtr _pP2S_YStep_Storge_Node = pP2S_YStep_Storge_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_YStep_Storge_Node,{_pP2S_ZIn_Storge_Node},"pP2S_YStep_Storge_Node",1);

    pP2S_Grab_Storge_Node = new GrabNode(GRABTYPE::type_Lossen,this);
    CGraph::GElementPtr _pP2S_Grab_Storge_Node = pP2S_Grab_Storge_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_Grab_Storge_Node,{_pP2S_YStep_Storge_Node},"pP2S_Grab_Storge_Node",1);

    pP2S_ZOut_Storge_Node = new ZMoveNode(ZMOVETYPE::type_Out,this);
    CGraph::GElementPtr _pP2S_ZOut_Storge_Node = pP2S_ZOut_Storge_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_ZOut_Storge_Node,{_pP2S_Grab_Storge_Node},"pP2S_ZOut_Storge_Node",1);

    pP2S_RA_Node = new RotateNode(ROTATETYPE::type_FaceA,this);
    CGraph::GElementPtr _pP2S_RA_Node = pP2S_RA_Node;
    status += pPort2StorgeLine->registerGNode(&_pP2S_RA_Node,{_pP2S_ZOut_Storge_Node},"pP2S_RA_Node",1);
    if (!status.isOK()) {
        mylog<<"build port to storge graph failed!msg:"<<QString::fromStdString(status.getInfo());
    }
    status.reset();

    /*取档案
        XY跑目标货位(->RB)->Z进->夹->Y抬升->Z出->XY跑至出货口->Z进->Y下降->松->Z出*/
    pStorge2PortLine = CGraph::GPipelineFactory::create();

    pS2P_XY_Storge_Node = new XYMoveNode(XYMOVETYPE::type_Storge,this);
    CGraph::GElementPtr _pS2P_XY_Storge_Node = pS2P_XY_Storge_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_XY_Storge_Node,{},"pS2P_XY_Storge_Node",1);

    pS2P_RB_Node = new RotateNode(ROTATETYPE::type_FaceB,this);
    CGraph::GElementPtr _pS2P_RB_Node  = pS2P_RB_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_RB_Node,{_pS2P_XY_Storge_Node},"pS2P_RB_Node",1);

    pS2P_Img_Storge_Node = new CameraNode(this);
    CGraph::GElementPtr _pS2P_Img_Storge_Node = pS2P_Img_Storge_Node;
    status = pStorge2PortLine->registerGNode(&_pS2P_Img_Storge_Node,{_pS2P_RB_Node},"_pP2S_Img_Port_Node",1);

    pS2P_ZIn_Port_Node = new ZMoveNode(ZMOVETYPE::type_In,this);
    CGraph::GElementPtr _pS2P_ZIn_Port_Node  = pS2P_ZIn_Port_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_ZIn_Port_Node,{_pS2P_Img_Storge_Node},"pS2P_ZIn_Port_Node",1);

    pS2P_Grab_Port_Node = new GrabNode(GRABTYPE::type_Clampe,this);
    CGraph::GElementPtr _pS2P_Grab_Port_Node  = pS2P_Grab_Port_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_Grab_Port_Node,{_pS2P_ZIn_Port_Node},"pS2P_Grab_Port_Node",1);

    pS2P_YStep_Port_Node = new YStepNode(YSTEPTYPE::type_UP,this);
    CGraph::GElementPtr _pS2P_YStep_Port_Node  = pS2P_YStep_Port_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_YStep_Port_Node,{_pS2P_Grab_Port_Node},"pS2P_YStep_Port_Node",1);

    pS2P_ZOut_Port_Node = new ZMoveNode(ZMOVETYPE::type_Out,this);
    CGraph::GElementPtr _pS2P_ZOut_Port_Node  = pS2P_ZOut_Port_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_ZOut_Port_Node,{_pS2P_YStep_Port_Node},"pS2P_ZOut_Port_Node",1);

    pS2P_RA_Node = new RotateNode(ROTATETYPE::type_FaceA,this);
    CGraph::GElementPtr _pS2P_RA_Node  = pS2P_RA_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_RA_Node,{_pS2P_ZOut_Port_Node},"pS2P_RA_Node",1);

    pS2P_XY_Port_Node = new XYMoveNode(XYMOVETYPE::type_Port,this);
    CGraph::GElementPtr _pS2P_XY_Port_Node  = pS2P_XY_Port_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_XY_Port_Node,{_pS2P_RA_Node},"pS2P_XY_Port_Node",1);

    pS2P_Img_Port_Node = new CameraNode(this);
    CGraph::GElementPtr _pS2P_Img_Port_Node = pS2P_Img_Port_Node;
    status = pStorge2PortLine->registerGNode(&_pS2P_Img_Port_Node,{_pS2P_XY_Port_Node},"_pP2S_Img_Port_Node",1);

    pS2P_ZIn_Storge_Node = new ZMoveNode(ZMOVETYPE::type_In,this);
    CGraph::GElementPtr _pS2P_ZIn_Storge_Node  = pS2P_ZIn_Storge_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_ZIn_Storge_Node,{_pS2P_Img_Port_Node},"pS2P_ZIn_Storge_Node",1);

    pS2P_YStep_Storge_Node = new YStepNode(YSTEPTYPE::type_DOWN,this);
    CGraph::GElementPtr _pS2P_YStep_Storge_Node  = pS2P_YStep_Storge_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_YStep_Storge_Node,{_pS2P_ZIn_Storge_Node},"pS2P_YStep_Storge_Node",1);

    pS2P_Grab_Storge_Node = new GrabNode(GRABTYPE::type_Lossen,this);
    CGraph::GElementPtr _pS2P_Grab_Storge_Node  = pS2P_Grab_Storge_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_Grab_Storge_Node,{_pS2P_YStep_Storge_Node},"pS2P_Grab_Storge_Node",1);

    pS2P_ZOut_Storge_Node = new ZMoveNode(ZMOVETYPE::type_Out,this);
    CGraph::GElementPtr _pS2P_ZOut_Storge_Node  = pS2P_ZOut_Storge_Node;
    status += pStorge2PortLine->registerGNode(&_pS2P_ZOut_Storge_Node,{_pS2P_Grab_Storge_Node},"pS2P_ZOut_Storge_Node",1);

    if (!status.isOK()) {
        mylog<<"build storge to port graph failed!msg:"<<QString::fromStdString(status.getInfo());
    }
}

void ServoManager::slot_InitDevice()
{
    pMyCanBus = new CanOpenBus();
    pMyCanBus->CanbusInit();

    connect(pMyCanBus,SIGNAL(sig_recvSDOFrame(CanopenSDOFrame)),this,SLOT(slot_recvSDOFrame(CanopenSDOFrame)));
    connect(pMyCanBus,SIGNAL(sig_recvPDOFrame(CanopenPDOFram)),this,SLOT(slot_recvPDOFrame(CanopenPDOFram)));
    //初始化步科伺服电机
    pXmotor = new KincoServo(pMyCanBus,XMOTOR_NODE,KINCOMOTORTYPE::ServoType);
    kincoMotorMap[XMOTOR_NODE] = pXmotor;
    pYmotor = new KincoServo(pMyCanBus,YMOTOR_NODE,KINCOMOTORTYPE::ServoType);
    kincoMotorMap[YMOTOR_NODE] = pYmotor;
    pZmotor = new KincoServo(pMyCanBus,ZMOTOR_NODE,KINCOMOTORTYPE::StepType);
    kincoMotorMap[ZMOTOR_NODE] = pZmotor;
    pRmotor = new KincoServo(pMyCanBus,RMOTOR_NODE,KINCOMOTORTYPE::ServoType);
    kincoMotorMap[RMOTOR_NODE] = pRmotor;

    //初始化夹爪电机
    pRmServo = new RmServo(pMyCanBus,GRAB_NODE);
    //每个电机进入MNT预操作状态
    SetServoAllMNT_PreOper();

    //设置每个电机的梯形速度，掉电不保存
    pXmotor->SetRwTrapeSpeed_SDO(100); //300000 ~ -200000
    pYmotor->SetRwTrapeSpeed_SDO(300);//xia -1550000 ~ shang -3950000 2.1100000 3.0
    pRmotor->SetRwTrapeSpeed_SDO(330);
    pZmotor->SetRwTrapeSpeed_SDO(350);

    //读每个电机的读写参数并上报
    pXmotor->ReadRoParams();
    pXmotor->ReadRwParams();

    pYmotor->ReadRoParams();
    pYmotor->ReadRwParams();

    pRmotor->ReadRoParams();
    pRmotor->ReadRwParams();

    pZmotor->ReadRoParams();
    pZmotor->ReadRwParams();
////    pKincoServo->SetRwCanHeartTime(1,3000);
////    pKincoServo->SetRwCanHeartTime(2,3000);

//    //每个电机进入MNT操作状态
    SetServoAllMNT_Oper();

    //初始化相机控制
    pCameraManager = new CameraManager();
    pCameraManager->slot_init();

    //单独X轴控制定时器
    ptimer_AxisX = new QTimer();
    connect(ptimer_AxisX,SIGNAL(timeout()),this,SLOT(slot_axisXTimeOut()));
    ptimer_AxisX->setInterval(1000);
    //单独Y轴控制定时器
    ptimer_AxisY = new QTimer();
    connect(ptimer_AxisY,SIGNAL(timeout()),this,SLOT(slot_axisYTimeOut()));
    ptimer_AxisY->setInterval(1000);
    //单独Z轴控制定时器
    ptimer_AxisZ = new QTimer();
    connect(ptimer_AxisZ,SIGNAL(timeout()),this,SLOT(slot_axisZTimeOut()));
    ptimer_AxisZ->setInterval(1000);
    //单独旋转轴控制定时器
    ptimer_Rotate = new QTimer();
    connect(ptimer_Rotate,SIGNAL(timeout()),this,SLOT(slot_rotateTimeOut()));
    ptimer_Rotate->setInterval(1000);
//    //找原点定时器
//    ptimer_SeekOrigin = new QTimer();
//    connect(ptimer_SeekOrigin,SIGNAL(timeout()),this,SLOT(slot_OriginTimeOut()));
//    ptimer_SeekOrigin->setInterval(1000);
//    //QR定位定时器
//    ptimer_QRLocate = new QTimer();
//    connect(ptimer_QRLocate, SIGNAL(timeout()), this, SLOT(slot_QRLocateTimeOut()));
//    ptimer_QRLocate->setInterval(1000);
    //定时上报每个电机只读参数控制器
    ptimer = new QTimer();
    connect(ptimer, SIGNAL(timeout()), this, SLOT(slot_timerOut()));
    ptimer->start(4000);

//    //构建业务控制逻辑图
//    /*存档案
//        XY跑至出货口->Z进->夹->Y抬升->Z出(->RB)->XY跑目标位置->Z进->Y下降->松->Z出
//    取档案
//        XY跑目标货位(->RB)->Z进->夹->Y抬升->Z出->XY跑至出货口->Z进->Y下降->松->Z出*/
    GraphBuild();

    //根据初始位置判定电机目前的位置 暂不起作用，上面读参数，跑到这里参数还没返回
    SetServoInitLocation();
    //开始上电找原点
    //SetStartAllSeekOrigin();
}

void ServoManager::slot_recvPDOFrame(CanopenPDOFram frame)
{
    uint8_t nodeid;
    uint32_t pdotype = frame.cobID & 0xFF0;
//    mylog<<"cobid:"<<hex<<pdotype;
    switch (pdotype) {
    // TPDO1 存放实际位置和实际速度
    case CANOPENCOBID::TPDO1_COBID:
    {
        nodeid = frame.cobID - CANOPENCOBID::TPDO1_COBID ;
        auto motorit = kincoMotorMap.find(nodeid);
        if(motorit != kincoMotorMap.end()){
            auto& motor = motorit->second;
            motor->mRo_RealPosition = (frame.data[3]<<24)|(frame.data[2]<<16)|(frame.data[1]<<8)|(frame.data[0]);
            motor->mRo_RealSpeed = motor->SpeedDec2Rpm((frame.data[7]<<24)|(frame.data[6]<<16)|(frame.data[5]<<8)|(frame.data[4]));
//            mylog<<nodeid<<" position:"<<motor->mRo_RealPosition<<" speed:"<<motor->mRo_RealSpeed;
        }
    }
        break;
    // TPDO2 存放实际电流 ,步进电机多一个实际总线电压
    case CANOPENCOBID::TPDO2_COBID:
    {
        nodeid = frame.cobID - CANOPENCOBID::TPDO2_COBID;
        if(nodeid != GRAB_NODE){
            auto motorit = kincoMotorMap.find(nodeid);
            if(motorit != kincoMotorMap.end()){
                auto& motor = motorit->second;
                motor->mRo_RealCurrent = motor->CurrentDec2Arms((frame.data[1]<<8)|(frame.data[0]));
                motor->mRo_RealSpeed = motor->SpeedDec2Rpm((frame.data[7]<<24)|(frame.data[6]<<16)|(frame.data[5]<<8)|(frame.data[4]));
                //步进独有
                motor->mRO_RealVoltage = (frame.data[3]<<8)|(frame.data[2]);
                //        mylog<<"position:"<<mRo_RealPosition<<" speed:"<<mRo_RealSpeed;
            }
        }
        else{
            mRm_IOStatus = frame.data[4];
        }
    }
        break;
    // TPDO3 存放状态字，警告状态字，实际有效模式
    case CANOPENCOBID::TPDO3_COBID:
    {
        nodeid = frame.cobID - CANOPENCOBID::TPDO3_COBID ;
//        mylog<<"nodeid"<<nodeid;
        auto motorit = kincoMotorMap.find(nodeid);
        if(motorit != kincoMotorMap.end()){
            auto& motor = motorit->second;
            motor->mRo_StatusWord = (frame.data[1]<<8)|(frame.data[0]);
            motor->mRo_WarnWord = (frame.data[3]<<8)|(frame.data[2]);
            motor->mRO_WorkMode = frame.data[4];
//            if(nodeid == 4)
//            {
//                if((pZmotor->mRo_StatusWord & KincoServo::Status_PowerOn) == 0)
//                    pZmotor->mRo_StatusWord = pZmotor->mRo_StatusWord & 0xFBFF;
//            }
            //        mylog<<"position:"<<mRo_RealPosition<<" speed:"<<mRo_RealSpeed;
        }
    }
        break;
    case CANOPENCOBID::Heart_COBID:
    {
        nodeid = frame.cobID - CANOPENCOBID::Heart_COBID ;
        auto motorit = kincoMotorMap.find(nodeid);
        if(motorit != kincoMotorMap.end()){
            auto& motor = motorit->second;
            if(motor->mHeartServo1 > 250)
                motor->mHeartServo1 = 3;
            else
                motor->mHeartServo1 += 3;
        }
    }
        break;
    }
}

void ServoManager::slot_recvSDOFrame(CanopenSDOFrame frame)
{
    if(frame.NodeID != GRAB_NODE){
//        mylog<<"frameid:"<<frame.NodeID;
        auto& motor = kincoMotorMap[frame.NodeID];
        if(motor){
            motor->analysisSDOParam(frame);
        }
    }
    else{

    }
}

/***************************************************************
* 函数名称：slot_SetServoParam
* 功能描述：通过SDO设置电机参数
* 参数说明：nodeID-节点号 objectindex-对象 value-值
* 返回值说明：
***************************************************************/
void ServoManager::slot_SetServoParam(uint8_t nodeID, int32_t objectindex, int32_t value)
{
    // 进入预操作状态，关闭所有电机的PDO
    //SetServoAllMNT_PreOper();

    auto motorit = kincoMotorMap.find(nodeID);
    if(motorit != kincoMotorMap.end()){
        auto& motor = motorit->second;
//        mylog<<"-----------setobj:"<<objectindex<<" ,value:"<<value;
        motor->SetServoParam(objectindex,value);
    }
}
/***************************************************************
* 函数名称：SetMoveXY
* 功能描述：通过PDO控制XY轴同时移动
* 参数说明：axisX-横坐标 axisY-纵坐标 movetype-类型，用于更新电机位置状态
* 返回值说明：
***************************************************************/
void ServoManager::SetMoveXY(int32_t axisX, int32_t axisY,uint8_t movetype)
{
//    mylog<<axisY;
      pXmotor->ArriveTargetPosition_PDO(axisX,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);//写3F立即根据目标位置移动
      pYmotor->ArriveTargetPosition_PDO(axisY,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);

      //更新Y
      mPreTargetY = axisY;

      //根据目标位置，更新XY电机状态
      if(movetype == ServoManager::Action_MoveXY2Origin)
      {
          pXmotor->mServoLocation = ServoManager::Location_Origining;
          pYmotor->mServoLocation = ServoManager::Location_Origining;
          mylog<<"update ServoXY origining";
      }
      else if(movetype == ServoManager::Action_MoveXY2PortTarget)
      {
          pXmotor->mServoLocation = ServoManager::Location_Porting;
          pYmotor->mServoLocation = ServoManager::Location_Porting;
          mylog<<"update ServoXY porting";
      }
      else if(movetype == ServoManager::Action_MoveXY2StorgeTarget)
      {
          pXmotor->mServoLocation = ServoManager::Location_Moving;
          pYmotor->mServoLocation = ServoManager::Location_Moving;
          mylog<<"update ServoXY moving";
      }
}
/***************************************************************
* 函数名称：SetMoveZ
* 功能描述：通过PDO控制Z轴移动
* 参数说明：axisZ-坐标
* 返回值说明：
***************************************************************/
void ServoManager::SetMoveZ(uint8_t type)
{
    if(type == ZMOVETYPE::type_In){
        pZmotor->ArriveTargetPosition_PDO(AxisZ_In,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);//写3F立即根据目标位置移动
    }
    //其他位置
    else{
        pZmotor->ArriveTargetPosition_PDO(AxisZ_Out,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);//写3F立即根据目标位置移动
    }
    pZmotor->mServoLocation = ServoManager::Location_Moving;
    mylog<<"update servoZ Location_Moving";
}

void ServoManager::SetMoveZbyValue(int32_t value)
{
    pZmotor->ArriveTargetPosition_PDO(value,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);//写3F立即根据目标位置移动
    pZmotor->mServoLocation = ServoManager::Location_Moving;
    mylog<<"update servoZ Location_Moving";
}
/***************************************************************
* 函数名称：SetMoveRotate
* 功能描述：通过PDO控制旋转
* 参数说明：angle-坐标，不是角度
* 返回值说明：
***************************************************************/
void ServoManager::SetMoveRotatebyValue(int32_t angle)
{
    pRmotor->ArriveTargetPosition_PDO(angle,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);//写3F立即根据目标位置移动

    //原点
    if(angle == 0)
    {
        pRmotor->mServoLocation = ServoManager::Location_Origining;
        mylog<<"update servoR origining";
    }
    //A面
    else if(angle == ServoManager::Rotate_FaceA_INC)
    {
        pRmotor->mServoLocation = ServoManager::Location_FaceAing;
        mylog<<"update servoR faceAing";
    }
    //B面
    else if(angle == ServoManager::Rotate_FaceB_INC)
    {
        pRmotor->mServoLocation = ServoManager::Location_FaceBing;
        mylog<<"update servoR faceBing";
    }
    //其他位置
    else
    {
        pRmotor->mServoLocation = ServoManager::Location_Moving;
        mylog<<"update servoR moving";
    }
}
/***************************************************************
* 函数名称：SetMoveClamp
* 功能描述：通过SDO控制夹爪
* 参数说明：clamp-RmServo::静态变量
* 返回值说明：
***************************************************************/
void ServoManager::SetMoveClamp(uint8_t clamp)
{
    if(clamp == GRABTYPE::type_Clampe)
    {
        pRmServo->SetMove45_SDO();
        pRmServo->mServoLocation = ServoManager::Location_Clamping;
        mylog<<"update servoM Clamping";
    }
    else if(clamp == GRABTYPE::type_Clampe)
    {
        pRmServo->SetMoveOrigin_SDO();
        pRmServo->mServoLocation = ServoManager::Location_Loosening;
        mylog<<"update servoM Loosening";
    }
    else
        return;
}

/***************************************************************
* 函数名称：SetYRiseFall
* 功能描述：通过PDO控制Y轴抬升下降
* 参数说明：type-true 抬升 false 下降
* 返回值说明：
***************************************************************/
void ServoManager::SetYStep(uint8_t type)
{
    //抬升
    if(type == YSTEPTYPE::type_UP)
    {
        pYmotor->ArriveTargetPosition_PDO(mPreTargetY-ServoManager::AxisY_RiseFall_INC,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);
        mylog<<"set Y rise "<<mPreTargetY<<" - "<<ServoManager::AxisY_RiseFall_INC;
        mPreTargetY = mPreTargetY-ServoManager::AxisY_RiseFall_INC;
    }
    //下降
    else
    {
        pYmotor->ArriveTargetPosition_PDO(mPreTargetY+ServoManager::AxisY_RiseFall_INC,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);
        mylog<<"set Y fall "<<mPreTargetY<<" + "<<ServoManager::AxisY_RiseFall_INC;;
        mPreTargetY = mPreTargetY+ServoManager::AxisY_RiseFall_INC;
    }
//        pYmotor->mServoLocation = ServoManager::Location_Moving;
//        mylog<<"update servoY moving";

}

/***************************************************************
* 函数名称：SetAllStop
* 功能描述：通过PDO下发所有电机断电指令
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetAllStop()
{

    pipeRunFlag = false;
    g_DebugCondition.wakeAll(); //唤醒处于调试模式的流程
    pPort2StorgeLine->cancel();
    pStorge2PortLine->cancel();

    pXmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
    pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
    pZmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
    pRmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
    ptimer_AxisX->stop();
    ptimer_AxisY->stop();
    ptimer_AxisZ->stop();
    ptimer_Rotate->stop();
//    ptimer_QRLocate->stop();
    mcnt_ActionType = ServoManager::Action_None;
    mcnt_ActionTime=0;
    mcnt_Rmaction=0;
    mcnt_QRLocateCnt = 0;
    mcnt_QRLocateType = QRLocateType_None;
    ArvCommand::getInstance().isExCompleted = true;
    QRLocateCommand::getInstance().isExCompleted = true;
    ArvCommand::getInstance().StepCnt = ServoManager::Debug_StepStatus_CmdOK;
}

void ServoManager::SetMoveRotate(uint8_t type)
{
    if(type == ROTATETYPE::type_FaceA){
        pRmotor->ArriveTargetPosition_PDO(Rotate_FaceA_INC,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);//写3F立即根据目标位置移动
        pRmotor->mServoLocation = ServoManager::Location_FaceAing;
    }
    else{
        pRmotor->ArriveTargetPosition_PDO(Rotate_FaceB_INC,KINCOCONTROLWORD::ControlWord_AbsoluteLocationStep2);//写3F立即根据目标位置移动
        pRmotor->mServoLocation = ServoManager::Location_FaceBing;
    }
}

/***************************************************************
* 函数名称：SetServoInitLocation
* 功能描述：伺服电机根据编码器位置判定目前的位置
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetServoInitLocation()
{
    //判断X轴位置
    if(pXmotor->mRo_RealPosition < ServoManager::Axis_Origin_INC+2000 && pXmotor->mRo_RealPosition > ServoManager::Axis_Origin_INC-2000)
    {
        pXmotor->mServoLocation = ServoManager::Location_Origin;
    }
    else
    {
        pXmotor->mServoLocation = ServoManager::Location_NoneOrigin;
    }
    //mylog<<"X init location "<<pXmotor->mServoLocation<<" "<<pXmotor->mRo_RealPosition;
    //判断Y轴位置
    if(pYmotor->mRo_RealPosition < ServoManager::Axis_Origin_INC+2000 && pYmotor->mRo_RealPosition > ServoManager::Axis_Origin_INC-2000)
    {
        pYmotor->mServoLocation = ServoManager::Location_Origin;
    }
    else
    {
        pYmotor->mServoLocation = ServoManager::Location_NoneOrigin;
    }
    //mylog<<"Y init location "<<pYmotor->mServoLocation<<" "<<pYmotor->mRo_RealPosition;
    //判断旋转电机位置
    if(pRmotor->mRo_RealPosition < ServoManager::Rotate_FaceA_INC+2000 && pRmotor->mRo_RealPosition > ServoManager::Rotate_FaceA_INC-2000)
    {
        pRmotor->mServoLocation = ServoManager::Location_FaceA;
    }
    else if(pRmotor->mRo_RealPosition < ServoManager::Axis_Origin_INC+2000 && pRmotor->mRo_RealPosition > ServoManager::Axis_Origin_INC-2000)
    {
        pRmotor->mServoLocation = ServoManager::Location_Origin;
    }
    else if(pRmotor->mRo_RealPosition < ServoManager::Rotate_FaceB_INC+2000 && pRmotor->mRo_RealPosition > ServoManager::Rotate_FaceB_INC-2000)
    {
        pRmotor->mServoLocation = ServoManager::Location_FaceB;
    }
    else
    {
        pRmotor->mServoLocation = ServoManager::Action_None;
    }
    mylog<<"R init location "<<pRmotor->mServoLocation<<" "<<pRmotor->mRo_RealPosition;
    //Z轴步进电机掉电不保存位置，需要根据旋转电机位置确定Z轴找原点模式，进行找原点
}
/***************************************************************
* 函数名称：SetOriginZ
* 功能描述：Z轴开始找原点，更新Z轴位置状态
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetSeekOriginZ()
{
    pZmotor->ArriveOriginPosition_PDO();
    pZmotor->mServoLocation = ServoManager::Location_Origining;
}
/***************************************************************
* 函数名称：SetOriginXY
* 功能描述：XY轴开始找原点，更新XY轴位置状态
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetSeekOriginXY()
{
    pXmotor->ArriveOriginPosition_PDO();
    pYmotor->ArriveOriginPosition_PDO();
    //如果当前位置就是原点，则原点信号找到本身就置位，会导致刚开始找原点就停止，所以先清除该位
    pXmotor->mRo_StatusWord = pXmotor->mRo_StatusWord & 0x7FFF;
    pYmotor->mRo_StatusWord = pYmotor->mRo_StatusWord & 0x7FFF;
    pXmotor->mServoLocation = ServoManager::Location_Origining;
    pYmotor->mServoLocation = ServoManager::Location_Origining;
}
/***************************************************************
* 函数名称：SetOriginR
* 功能描述：R轴开始找原点，更新R轴位置状态
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetSeekOriginR()
{
    pRmotor->ArriveOriginPosition_PDO();
    pRmotor->mServoLocation = ServoManager::Location_Origining;
}
/***************************************************************
* 函数名称：SetStartAllSeekOrigin
* 功能描述：开始上电找原点，根据旋转电机的位置选择Z轴找原点模式，开始找原点计时
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetStartAllSeekOrigin()
{
    if(pRmotor->mServoLocation == ServoManager::Location_FaceB){
        pRmotor->SetRwOriginMode_SDO(24);
    }
    SetSeekOriginZ();
    mcnt_ActionType = ServoManager::Action_ZFindOrigin;
    ptimer_SeekOrigin->start();
}

void ServoManager::SendLocateResult()
{
    QJsonObject json;
    json.insert("CmdType","QRLOCATE");
    json.insert("LocateResult",mIsFindLocator);
    json.insert("LocateType",mcnt_QRLocateType);
    json.insert("LocateCnt",mcnt_QRLocateCnt);
    json.insert("LocatePosition",mLocatorPosition);
    mylog<<json;
    emit sig_SendServoData(json);
}
/***************************************************************
* 函数名称：slot_ReadRwParams
* 功能描述：读电机的读写参数
* 参数说明：nodeID-节点号
* 返回值说明：
***************************************************************/
void ServoManager::slot_ReadRwParams(uint8_t nodeID)
{
    QJsonObject roParams;
    auto motorit = kincoMotorMap.find(nodeID);
    if(motorit != kincoMotorMap.end()){
        auto& motor = motorit->second;
        QJsonObject jsonData;
        jsonData.insert("CmdType","PROBE_RW");
        jsonData.insert("DeviceNo",nodeID);
        jsonData.insert("WorkMode", motor->mRW_WorkMode);     //工作模式
        jsonData.insert("ControlWord", motor->mRW_ControlWord);  //控制字
        jsonData.insert("TargetPosition", motor->mRW_TargetPosition);   //目标位置
        jsonData.insert("TrapeSpeed", (int32_t)motor->mRW_TrapeSpeed);   //梯形速度
        jsonData.insert("TrapeAcc", (int32_t)motor->mRW_TrapeAcc); //梯形加速度
        jsonData.insert("TrapeDec", (int32_t)motor->mRW_TrapeDec); //梯形减速度
        jsonData.insert("TargetSpeed", motor->mRW_TargetSpeed);  //目标速度
        jsonData.insert("TargetTorque", motor->mRW_TargetTorque); //目标扭矩
        jsonData.insert("EncoderReset",motor->mRW_EncoderReset) ; //编码器数据复位
        jsonData.insert("CanRate", motor->mRW_CanRate); //can波特率
        jsonData.insert("CanHeartTime", motor->mRW_CanHeartTime); //can心跳报文产生时间
        jsonData.insert("PositiveLimit", motor->mRO_PositiveLimit); //正限位位置
        jsonData.insert("NagetiveLimit", motor->mRO_NagetiveLimit); //负限位位置
        jsonData.insert("OriginMode",motor->mRW_OriginMode);
        jsonData.insert("BreakEnable",motor->mRW_ServoBreak);  //垂直电机额外读取电机附件参数，看是否设置了报闸
        mylog<<roParams;
        emit sig_SendServoData(roParams);
    }
    //补充异常情况

}
/***************************************************************
* 函数名称：slot_MoveCmd
* 功能描述：解析上位机移动命令指令并执行
* 参数说明：cmd-上位机下发的指令
* 返回值说明：
***************************************************************/
void ServoManager::slot_MoveCmd(QJsonObject cmd)
{
    QString movetype = cmd.value("MoveType").toString();
    QString moveword = cmd.value("MoveWord").toString();
    if(movetype == "MOVEXY")
    {
        int32_t axisX = cmd.value("AxisX").toInt();
        int32_t axisY = cmd.value("AxisY").toInt();
        //通过PDO下发指令
        SetMoveXY(axisX,axisY,ServoManager::Action_MoveXY2StorgeTarget);
        //启动定时器
        ptimer_AxisX->start();
        ptimer_AxisY->start();
        mylog<<"XY Timer Start!";
    }
    else if(movetype == "MOVEZ")
    {
        if(moveword == "In"){
            SetMoveZ(ZMOVETYPE::type_In);
        }
        else if(moveword == "Out"){
            SetMoveZ(ZMOVETYPE::type_Out);
        }
        else if(moveword == ""){
            SetMoveZbyValue(cmd.value("AxisZ").toInt());
        }
        else
            return;

        //启动定时器
        ptimer_AxisZ->start();
        mylog<<"Z Timer Start!";
    }
    else if(movetype == "MOVEROTATE")
    {
        int32_t rotate;
        if(moveword == "Origin")
            rotate = 0;
        else if(moveword == "FaceA")
            rotate = ServoManager::Rotate_FaceA_INC;
        else if(moveword == "FaceB")
            rotate = ServoManager::Rotate_FaceB_INC;
        else if(moveword == ""){
            rotate = cmd.value("Angle").toInt();
        }
        else
            return;
        SetMoveRotatebyValue(rotate);
        //启动定时器
        ptimer_Rotate->start();
        mylog<<"Rotate Timer Start!";
    }
    else if(movetype == "MOVECLAMP")
    {
        uint8_t clamp = cmd.value("Clamp").toInt();
        //通过SDO下发指令
        SetMoveClamp(clamp);
    }
    else if(movetype == "MOVERISE")
    {
        SetYStep(YSTEPTYPE::type_UP);
        ptimer_AxisY->start();
        mylog<<"Y timer start!";
    }
    else if(movetype == "MOVEFALL")
    {
        SetYStep(YSTEPTYPE::type_DOWN);
        ptimer_AxisY->start();
        mylog<<"Y timer start!";
    }
    else if(movetype == "MOVESTOP")
    {
        if(moveword == "STOP"){
            SetAllStop();
            mylog<<"set all stop!";
        }
        else{
                pipeRunFlag = true;
        }
    }
    else
        return;

}

void ServoManager::slot_FindLocatorDistance(bool isfind, int distance)
{
     mIsFindLocator = isfind;
     mLocatorDistance = distance;
     mcnt_ActionType = Action_QRScaned;
}

void ServoManager::slot_FindTargetArvQR(bool isfind, int distance)
{
    mIsFindTargetQR = isfind;
    mTargetArvDistance = distance;
    mcnt_ActionType = Action_QRScaned;
}
/***************************************************************
* 函数名称：slot_startInOutCabCmd
* 功能描述：通过指令下发出入柜动作并启动action定时器
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::slot_startInOutCabCmd()
{
    //急停后，必须先接触急停
    if(!pipeRunFlag)    return;
    //根据存或取判定先去货位还是取货口
    if(ArvCommand::getInstance().Type == ARVCMDTYPE::type_incab)
    {
        pP2S_XY_Port_Node->setXY(ArvCommand::getInstance().PortLocation_X,ArvCommand::getInstance().PortLocation_Y);
        pP2S_XY_Storge_Node->setXY(ArvCommand::getInstance().StorgeLocation_X,ArvCommand::getInstance().StorgeLocation_Y-ServoManager::AxisY_RiseFall_INC);
        if(ArvCommand::getInstance().Face == ARVCMDFACE::face_b){
            pP2S_RA_Node->setEnable(true);
            pP2S_RB_Node->setEnable(true);
        }
        else{
            pP2S_RA_Node->setEnable(false);
            pP2S_RB_Node->setEnable(false);
        }
        mcnt_ActionType = ServoManager::Action_MoveXY2PortTarget;
        mylog<<"start pPort2StorgeLine flow,Set servoXY to port";
        std::thread processThread([this]() {
            auto ret = pPort2StorgeLine->asyncProcess();
            ret.wait();
            ArvCommand::getInstance().isExCompleted = true;
        });

        processThread.detach(); // 分离线程，使其在后台运行
    }
    else
    {
        pS2P_XY_Port_Node->setXY(ArvCommand::getInstance().PortLocation_X,ArvCommand::getInstance().PortLocation_Y-ServoManager::AxisY_RiseFall_INC);
        pS2P_XY_Storge_Node->setXY(ArvCommand::getInstance().StorgeLocation_X,ArvCommand::getInstance().StorgeLocation_Y);
        if(ArvCommand::getInstance().Face == ARVCMDFACE::face_b){
            pS2P_RA_Node->setEnable(true);
            pS2P_RB_Node->setEnable(true);
        }
        else{
            pS2P_RA_Node->setEnable(false);
            pS2P_RB_Node->setEnable(false);
        }

        mcnt_ActionType = ServoManager::Action_MoveXY2StorgeTarget;
        mylog<<"start pStorge2PortLine flow,Set servoXY to storge";
        std::thread processThread([this]() {
            auto ret = pStorge2PortLine->asyncProcess();
            ret.wait();
            ArvCommand::getInstance().isExCompleted = true;
        });

        processThread.detach(); // 分离线程，使其在后台运行
    }
}

void ServoManager::slot_startQRLocateCmd()
{
    //应先判断每个电机是不是在原点

    mcnt_QRLocateType = ServoManager::QRLocateType_X;
    //更新动作类型
    mcnt_ActionType = ServoManager::Action_XLocate;
    mcnt_QRLocateCnt = 1;//先定位出货口所在层第一节
    //根据小节尺寸计算新的X位置
    int subpostion = QRLocateCommand::getInstance().subWidth[mcnt_QRLocateCnt - 1]*AxisX_1mm_INC;
    //下指令
    SetMoveXY(subpostion,Axis_Origin_INC,Action_MoveXY2StorgeTarget);
    QRLocateCommand::getInstance().firstSubPosition = subpostion;
    mLocatorPosition = subpostion;
    mylog<<"set qrlocatetype QRLocateType_X, actiontype Action_XLocate, moveXposition:"<<subpostion;
    ptimer_QRLocate->start();
}

void ServoManager::slot_imgCmd()
{
    QImage img;
    if(pCameraManager->slot_catchImage(img)){

        emit sig_imgResultToServer(img);
//        QZXing decoder;
//            //mandatory settings
//        decoder.setDecoder( QZXing::DecoderFormat_QR_CODE | QZXing::DecoderFormat_EAN_13 );

//        //optional settings
//        //decoder.setSourceFilterType(QZXing::SourceFilter_ImageNormal | QZXing::SourceFilter_ImageInverted);
//        decoder.setSourceFilterType(QZXing::SourceFilter_ImageNormal);
//        decoder.setTryHarderBehaviour(QZXing::TryHarderBehaviour_ThoroughScanning | QZXing::TryHarderBehaviour_Rotate);

//         //trigger decode
//        QString result = decoder.decodeImage(img);
//        mylog<<"decode qrcode:"<<result;
//        QJsonObject jsonData;
//        jsonData.insert("CmdType","PROBE_RW");
//        jsonData.insert("QRCode",result);
//        emit sig_SendServoData(jsonData);
    }
    else{
        mylog<<"catch one image failed!";
    }
}
/***************************************************************
* 函数名称：SetServoMNT_PreOper
* 功能描述：MNT进入预操作状态
* 参数说明：nodeid-节点号
* 返回值说明：
***************************************************************/
bool ServoManager::SetServoMNT_PreOper(uint8_t nodeid)
{
    bool result = false;
    auto& motor = kincoMotorMap[nodeid];
    if(motor){
        result = motor->SetMNTStatus(KINCOMNT::MNTCommand_PreOperation);
    }
    return result;
}
/***************************************************************
* 函数名称：SetServoAllMNT_PreOper
* 功能描述：设置所有电机进入预操作状态
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetServoAllMNT_PreOper()
{
    SetServoMNT_PreOper(mServoNo[0]);
    SetServoMNT_PreOper(mServoNo[1]);
    SetServoMNT_PreOper(mServoNo[2]);
    SetServoMNT_PreOper(mServoNo[3]);
}
/***************************************************************
* 函数名称：SetServoMNT_Oper
* 功能描述：MNT进入操作状态
* 参数说明：nodeid-节点号
* 返回值说明：
***************************************************************/
bool ServoManager::SetServoMNT_Oper(uint8_t nodeid)
{
    bool result = false;
    auto& motor = kincoMotorMap[nodeid];
    if(motor){
        result = motor->SetMNTStatus(KINCOMNT::MNTCommand_OpenNode);
    }
    return result;
}
/***************************************************************
* 函数名称：SetServoAllMNT_Oper
* 功能描述：设置所有电机进入操作状态
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::SetServoAllMNT_Oper()
{
    SetServoMNT_Oper(mServoNo[0]);
    QThread::msleep(50);  //延时避免五个电机同时开启MNT导致总线负载过大
    SetServoMNT_Oper(mServoNo[1]);
    QThread::msleep(50);
    SetServoMNT_Oper(mServoNo[2]);
    QThread::msleep(50);
    SetServoMNT_Oper(mServoNo[3]);
    QThread::msleep(50);
}

/***************************************************************
* 函数名称：slot_timerOut
* 功能描述：定时上报电机只读状态，心跳检测
* 参数说明：
* 返回值说明：
***************************************************************/
void ServoManager::slot_timerOut()
{
    for(auto& kv:kincoMotorMap){
        if(kv.second->mHeartServo1>0)
            kv.second->mHeartServo1 -= 1;
    }

    kincoMotorMap[mcnt]->ReadRoParams();
    kincoMotorMap[mcnt]->ReadRwParams();

    int shangbaoid = mcnt!=1?mcnt-1:4;

    //mcnt = 3;
    QJsonObject jsonData;

    jsonData.insert("CmdType","PROBE");
    jsonData.insert("DeviceNo",kincoMotorMap[shangbaoid]->nodeid);
    jsonData.insert("RoWorkMode", kincoMotorMap[shangbaoid]->mRO_WorkMode); //工作模式
    jsonData.insert("RoStateWord", kincoMotorMap[shangbaoid]->mRo_StatusWord); //状态字
    jsonData.insert("RoRealPosition", kincoMotorMap[shangbaoid]->mRo_RealPosition);     //实际位置
    jsonData.insert("RoRealSpeed", kincoMotorMap[shangbaoid]->mRo_RealSpeed);    //实际速度
    jsonData.insert("RoRealCurrent", kincoMotorMap[shangbaoid]->mRo_RealCurrent);  //实际电流
    jsonData.insert("RoWarnStateWord", kincoMotorMap[shangbaoid]->mRo_WarnWord);    //警告状态字
    jsonData.insert("RoHeart",kincoMotorMap[shangbaoid]->mHeartServo1);  //心跳，判断是否在线
    jsonData.insert("RoRealVoltage",kincoMotorMap[shangbaoid]->mRO_RealVoltage);

    jsonData.insert("RwWorkMode", kincoMotorMap[shangbaoid]->mRW_WorkMode);     //工作模式
    jsonData.insert("RwControlWord", kincoMotorMap[shangbaoid]->mRW_ControlWord);  //控制字
    jsonData.insert("RwTargetPosition", kincoMotorMap[shangbaoid]->mRW_TargetPosition);   //目标位置
    jsonData.insert("RwTrapeSpeed", (int32_t)kincoMotorMap[shangbaoid]->mRW_TrapeSpeed);   //梯形速度
    jsonData.insert("RwTrapeAcc", (int32_t)kincoMotorMap[shangbaoid]->mRW_TrapeAcc); //梯形加速度
    jsonData.insert("RwTrapeDec", (int32_t)kincoMotorMap[shangbaoid]->mRW_TrapeDec); //梯形减速度
    jsonData.insert("RwTargetSpeed", kincoMotorMap[shangbaoid]->mRW_TargetSpeed);  //目标速度
    jsonData.insert("RwTargetTorque", kincoMotorMap[shangbaoid]->mRW_TargetTorque); //目标扭矩
    jsonData.insert("RwEncoderReset",kincoMotorMap[shangbaoid]->mRW_EncoderReset) ; //编码器数据复位
    jsonData.insert("RwCanRate", kincoMotorMap[shangbaoid]->mRW_CanRate); //can波特率
    jsonData.insert("RwCanHeartTime", kincoMotorMap[shangbaoid]->mRW_CanHeartTime); //can心跳报文产生时间
    jsonData.insert("RwPositiveLimit", kincoMotorMap[shangbaoid]->mRO_PositiveLimit); //正限位位置
    jsonData.insert("RwNagetiveLimit", kincoMotorMap[shangbaoid]->mRO_NagetiveLimit); //负限位位置
    jsonData.insert("RwOriginMode",kincoMotorMap[shangbaoid]->mRW_OriginMode);
    jsonData.insert("RwBreakEnable",kincoMotorMap[shangbaoid]->mRW_ServoBreak);  //垂直电机额外读取电机附件参数，看是否设置了报闸
    //将五个电机位置状态转换成字符串，节省传数量
    QString location;
    for(qint8 i=1;i<5;i++)
    {
        location += (QString::number(kincoMotorMap[i]->mServoLocation)+",");
    }
    jsonData.insert("Location",location);
//    mylog<<jsonData;
    emit sig_SendServoData(jsonData);
    if(++mcnt>=5)
        mcnt = 1;
}
/*A面 存
    XY跑至出货口->Z进->夹->Y抬升->Z出->XY跑目标位置->Z进->Y下降->松->Z出
A面 取
    XY跑目标货位->Z进->夹->Y抬升->Z出->XY跑至出货口->Z进->Y下降->松->Z出
B面 存
    XY跑至出货口->Z进->夹->Y抬升->Z出->RB->XY跑目标位置->Z进->Y下降->松->Z出->RA
B面 取
    XY跑目标货位->RB->Z进->夹->Y抬升->Z出->XY跑至出货口->RA->Z进->Y下降->松->Z出->旋转R至A面*/

void ServoManager::slot_axisYTimeOut()
{
    //如果2号电机目标位置到
    if((pYmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) != 0)
    {
        //发送PDO控制字06断电
        pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
        ptimer_AxisY->stop();
        mylog<<"Y Timer Stop!";

        if(pYmotor->mServoLocation == ServoManager::Location_Moving)
        {
            pYmotor->mServoLocation = ServoManager::Location_NoneOrigin;
            mylog<<"update servoX NoneOrigin";
        }
        else if(pYmotor->mServoLocation == ServoManager::Location_Origining)
        {
            pYmotor->mServoLocation = ServoManager::Location_Origin;
            mylog<<"update servoX Origin";
        }
    }
}

void ServoManager::slot_axisXTimeOut()
{
    //如果1号电机目标位置到
    if((pXmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) != 0)
    {
        //发送PDO控制字06断电
        pXmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
        ptimer_AxisX->stop();
        mylog<<"X Timer Stop!";
        if(pXmotor->mServoLocation == ServoManager::Location_Moving)
        {
            pXmotor->mServoLocation = ServoManager::Location_NoneOrigin;
            mylog<<"update servoX NoneOrigin";
        }
        else if(pXmotor->mServoLocation == ServoManager::Location_Origining)
        {
            pXmotor->mServoLocation = ServoManager::Location_Origin;
            mylog<<"update servoX Origin";
        }
    }
}

void ServoManager::slot_axisZTimeOut()
{
    //mylog<<(pZmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive);
    //如果4号电机目标位置到
    if((pZmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) != 0)
    {
        //发送PDO控制字06断电
        pZmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
        ptimer_AxisZ->stop();
        mylog<<"Z Timer Stop!";

        if(pZmotor->mServoLocation == ServoManager::Location_FaceAing)
        {
            pZmotor->mServoLocation = ServoManager::Location_FaceA;
            mylog<<"update servoZ FaceA";
        }
        else if(pZmotor->mServoLocation == ServoManager::Location_FaceBing)
        {
            pZmotor->mServoLocation = ServoManager::Location_FaceB;
            mylog<<"update servoZ FaceB";
        }
        else if(pZmotor->mServoLocation == ServoManager::Location_Origining)
        {
            pZmotor->mServoLocation = ServoManager::Location_Origin;
            mylog<<"update servoZ Origin";
        }
        else
        {
            pZmotor->mServoLocation = ServoManager::Location_NoneOrigin;
            mylog<<"update servoZ NoneOrigin";
        }
    }
}

void ServoManager::slot_rotateTimeOut()
{
    //如果3号电机目标位置到
    if((pRmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) != 0)
    {
        //发送PDO控制字06断电
        pRmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
        ptimer_Rotate->stop();
        mylog<<"Rotate Timer Stop!";

        if(pRmotor->mServoLocation == ServoManager::Location_FaceAing)
        {
            pRmotor->mServoLocation = ServoManager::Location_FaceA;
            mylog<<"update servoR FaceA";
        }
        else if(pRmotor->mServoLocation == ServoManager::Location_FaceBing)
        {
            pRmotor->mServoLocation = ServoManager::Location_FaceB;
            mylog<<"update servoR FaceB";
        }
        else if(pRmotor->mServoLocation == ServoManager::Location_Origining)
        {
            pRmotor->mServoLocation = ServoManager::Location_Origin;
            mylog<<"update servoR Origin";
        }
        else
        {
            pRmotor->mServoLocation = ServoManager::Location_NoneOrigin;
            mylog<<"update servoZ NoneOrigin";
        }
    }
}

void ServoManager::slot_OriginTimeOut()
{
    //mylog<<mcnt_ActionType<<" "<<pZmotor->mRo_StatusWord;
    switch (mcnt_ActionType) {
    case ServoManager::Action_ZFindOrigin:
        if((pZmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_OriginFind) != 0)
        {
            mylog<<"Axis Z Origin find!";
            //Z轴定位原点后，旋转电机R开始定位原点
            //发送PDO控制字06断电
            pZmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            //更新Z轴位置
            pZmotor->mServoLocation = ServoManager::Location_Origin;
            //更新动作类型
            mcnt_ActionType = ServoManager::Action_RFindOrigin;
            //下指令
            SetSeekOriginR();
        }
        break;
    case ServoManager::Action_RFindOrigin:
        if((pRmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_OriginFind) != 0)
        {
            mylog<<"Servo R Origin find!";
            //R轴定位原点后，旋转到A面
            //发送PDO控制字06断电
            pRmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            //更新R位置
            pRmotor->mServoLocation = ServoManager::Location_Origin;
            //更新动作类型
            mcnt_ActionType = ServoManager::Action_Rotate;
            //下指令
            SetMoveRotate(ServoManager::Rotate_FaceA_INC);
        }
        break;
    case ServoManager::Action_Rotate:
        if((pRmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) != 0)
        {
            //R旋转到A面，XY定位原点
            //发送PDO控制字06断电
            pRmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            //更新R位置
            pRmotor->mServoLocation = ServoManager::Location_FaceA;
            //更新动作类型
            mcnt_ActionType = ServoManager::Action_XYFindOrigin;
            //下指令
            SetSeekOriginXY();
        }
        break;
    case ServoManager::Action_XYFindOrigin:
        if((pXmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_OriginFind) != 0 && (pYmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_OriginFind) != 0)
        {
            mylog<<"Axis XY Origin find!";
            //XY找到原点后停止
            //发送PDO控制字06断电
            pXmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            //更新电机位置状态
            pXmotor->mServoLocation = Location_Origin;
            pYmotor->mServoLocation = Location_Origin;
            //停止计时
            mcnt_ActionType = Action_None;
            ptimer_SeekOrigin->stop();
            mylog<<"time origin stop!";
        }
        break;
    }

}

void ServoManager::slot_QRLocateTimeOut()
{
    if(mcnt_ActionType == Action_XLocate)
    {
        //若1号 2号电机目标位置到
        if((pXmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0 && (pYmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0 )
        {
            //如果是非调试状态则直接执行，调试状态要doing状态下才行，避免调试状态下断电后目标位置到没有及时更改而重复执行
            if(QRLocateCommand::getInstance().isDebug == false || (QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdDoing))
            {
                mylog<<"Action_XLocate Done";
                //电机断电
                pXmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
                pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
                //更新电机位置状态
                pXmotor->mServoLocation = ServoManager::Location_NoneOrigin;
                pYmotor->mServoLocation = ServoManager::Location_NoneOrigin;
                mylog<<"update servoXY NoneOrigin";
                //如果是调试状态则不执行下一步，等到上位机指令
                if(QRLocateCommand::getInstance().isDebug == false)
                {
                    mcnt_ActionType = Action_QRScaning;
                    mIsFindLocator = false;
                    //发送信号启动二维码扫描线程
                    emit sig_StartQRLocateScan(QRLocateType_X,mcnt_QRLocateCnt);
                    mylog<<"start QRScan! type: locate_x, locatorNo:"<<mcnt_QRLocateCnt;
                }
                else
                {
                    //调试状态，更新步骤状态变量
                    QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
                    mylog<<"wait for next cmd....";
                }
            }
        }
        //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
        if(QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdNext)
        {
            QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
            mcnt_ActionType = Action_QRScaning;
            mIsFindLocator = false;
            //发送信号启动二维码扫描线程
            emit sig_StartQRLocateScan(QRLocateType_X,mcnt_QRLocateCnt);
            mylog<<"start QRScan! type: locate_x, locatorNo:"<<mcnt_QRLocateCnt;
        }
    }
    else if(mcnt_ActionType == Action_YLocate)
    {
        //若1号 2号电机目标位置到
        if((pXmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0 && (pYmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0 )
        {
            //如果是非调试状态则直接执行，调试状态要doing状态下才行，避免调试状态下断电后目标位置到没有及时更改而重复执行
            if(QRLocateCommand::getInstance().isDebug == false || (QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdDoing))
            {
                mylog<<"Action_YLocate Done";
                //电机断电
                pXmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
                pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
                //更新电机位置状态
                pXmotor->mServoLocation = ServoManager::Location_NoneOrigin;
                pYmotor->mServoLocation = ServoManager::Location_NoneOrigin;
                mylog<<"update servoXY NoneOrigin";
                //如果是调试状态则不执行下一步，等到上位机指令
                if(QRLocateCommand::getInstance().isDebug == false)
                {
                    mcnt_ActionType = Action_QRScaning;
                    mIsFindLocator = false;
                    //发送信号启动二维码扫描线程
                    emit sig_StartQRLocateScan(QRLocateType_Y,QRLocateCommand::getInstance().totalSub+mcnt_QRLocateCnt);
                    mylog<<"start QRScan! type: locate_Y, locatorNo:"<<QRLocateCommand::getInstance().totalSub+mcnt_QRLocateCnt;
                }
                else
                {
                    //调试状态，更新步骤状态变量
                    QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
                    mylog<<"wait for next cmd....";
                }
            }
        }
        //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
        if(QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdNext)
        {
            QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
            mcnt_ActionType = Action_QRScaning;
            mIsFindLocator = false;
            //发送信号启动二维码扫描线程
            emit sig_StartQRLocateScan(QRLocateType_Y,QRLocateCommand::getInstance().totalSub+mcnt_QRLocateCnt);
            mylog<<"start QRScan! type: locate_Y, locatorNo:"<<QRLocateCommand::getInstance().totalSub+mcnt_QRLocateCnt;
        }
    }
    else if(mcnt_ActionType == Action_QRScaning)
    {

    }
    else if(mcnt_ActionType == Action_QRScaned)
    {
        //如果是非调试状态则直接执行，调试状态要doing状态下才行，避免调试状态下断电后目标位置到没有及时更改而重复执行
        if(QRLocateCommand::getInstance().isDebug == false || (QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdDoing))
        {
            mylog<<"Action_QRScaned done!";
            //如果是调试状态则不执行下一步，等到上位机指令
            if(QRLocateCommand::getInstance().isDebug == false)
            {
                //回原点,回原点之后再判断误差
                SetMoveXY(Axis_Origin_INC,Axis_Origin_INC,Action_MoveXY2Origin);
                mcnt_ActionType = Action_MoveXY2Origin;
                mylog<<"set servoxy origin!";
            }
            else
            {
                //调试状态，更新步骤状态变量
                QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
                mylog<<"wait for next cmd....";
            }
        }
        //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
        if(QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdNext)
        {
            QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
            //回原点,回原点之后再判断误差
            SetMoveXY(Axis_Origin_INC,Axis_Origin_INC,Action_MoveXY2Origin);
            mcnt_ActionType = Action_MoveXY2Origin;
            mylog<<"set servoxy origin!";
        }
    }
    else if(mcnt_ActionType == Action_MoveXY2Origin)
    {
        //若1号 2号电机目标位置到
        if((pXmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0 && (pYmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0 )
        {
            //如果是非调试状态则直接执行，调试状态要doing状态下才行，避免调试状态下断电后目标位置到没有及时更改而重复执行
            if(QRLocateCommand::getInstance().isDebug == false || (QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdDoing))
            {
                mylog<<"Action_MoveXY2Origin Done";
                //电机断电
                pXmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
                pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
                //更新电机位置状态
                pXmotor->mServoLocation = ServoManager::Location_Origin;
                pYmotor->mServoLocation = ServoManager::Location_Origin;
                mylog<<"update servoXY Origin";
                //如果是调试状态则不执行下一步，等到上位机指令
                if(QRLocateCommand::getInstance().isDebug == false)
                {
                    //如果找到相应的定位QR码
                    if(mIsFindLocator == true)
                    {
                        //如果偏差距离小于设定误差，则上报上位机，并运动到下一个定位位置
                        if(mLocatorDistance < QRLocateError)
                        {
                            mylog<<"locate error"<<mLocatorDistance<<" ok!";
                            //发送信号上报上位机

                            if(mcnt_QRLocateType == QRLocateType_X)
                            {
                                if(mcnt_QRLocateCnt < QRLocateCommand::getInstance().totalSub)
                                {
                                    mcnt_QRLocateCnt++;
                                    //更新动作类型
                                    mcnt_ActionType = Action_XLocate;
                                    //根据小节尺寸计算新的X位置
                                    int subpostion = QRLocateCommand::getInstance().subWidth[mcnt_QRLocateCnt - 1]*AxisX_1mm_INC;
                                    //下指令
                                    SetMoveXY(subpostion,Axis_Origin_INC,Action_MoveXY2StorgeTarget);
                                    mylog<<"set actiontype Action_XLocate, moveXposition:"<<subpostion;
                                }
                                else if(mcnt_QRLocateCnt == QRLocateCommand::getInstance().totalSub)
                                {
                                    mcnt_QRLocateCnt = 1;
                                    //更新动作类型
                                    mcnt_ActionType = Action_YLocate;
                                    mcnt_QRLocateType = QRLocateType_Y;
                                    //根据小节尺寸计算下一层的位置
                                    int layerpostion = (QRLocateCommand::getInstance().portLayer - mcnt_QRLocateCnt) * AxisY_Layer_INC;
                                    //下指令
                                    SetMoveXY(Axis_Origin_INC,layerpostion,Action_MoveXY2StorgeTarget);
                                    mylog<<"set qrlocatetype QRLocateType_Y, actiontype Action_YLocate, moveXposition:"<<layerpostion;
                                }
                            }
                            if(mcnt_QRLocateType == QRLocateType_Y)
                            {
                                if(mcnt_QRLocateCnt < QRLocateCommand::getInstance().totalLayer)
                                {
                                    mcnt_QRLocateCnt++;
                                    //跳过出货口那一层定位，原点已经定位
                                    if(mcnt_QRLocateCnt == QRLocateCommand::getInstance().portLayer)
                                        mcnt_QRLocateCnt++;
                                    //更新动作类型
                                    mcnt_ActionType = Action_YLocate;
                                    //根据小节尺寸计算层的位置
                                    int layerpostion = (QRLocateCommand::getInstance().portLayer - mcnt_QRLocateCnt) * AxisY_Layer_INC;
                                    //下指令//下指令
                                    SetMoveXY(QRLocateCommand::getInstance().firstSubPosition,layerpostion,Action_MoveXY2StorgeTarget);
                                    mylog<<"set actiontype Action_YLocate, moveXposition:"<<layerpostion;
                                }
                                else if(mcnt_QRLocateCnt == QRLocateCommand::getInstance().totalLayer)
                                {
                                    //结束
                                    mcnt_ActionType = Action_None;
                                    mcnt_QRLocateCnt = 0;
                                    mcnt_QRLocateType = QRLocateType_None;
                                    ptimer_QRLocate->stop();
                                    mylog<<"QRlocate done!";
                                }
                            }
                        }
                        //如果误差大于设定值，则加上误差距离后重新运动至定位点
                        else
                        {
                            if(mcnt_QRLocateType == QRLocateType_X)
                            {
                                //根据距离换算成实际的编码器值
                                int inc = QRLocateCommand::getInstance().subWidth[mcnt_QRLocateCnt-1] + mLocatorDistance;
                                SetMoveXY(inc,Axis_Origin_INC,Action_MoveXY2StorgeTarget);
                                mcnt_ActionType = Action_XLocate;
                                 mylog<<"set actiontype Action_XLocate, moveXposition:"<<inc;
                            }
                            else
                            {
                                //根据距离换算成实际的编码器值
                                int inc = (QRLocateCommand::getInstance().portLayer - mcnt_QRLocateCnt) * AxisY_Layer_INC - mLocatorDistance;
                                SetMoveXY(QRLocateCommand::getInstance().firstSubPosition,inc,Action_MoveXY2StorgeTarget);
                                mcnt_ActionType = Action_YLocate;
                                mylog<<"set actiontype Action_YLocate, moveXposition:"<<inc;
                            }
                        }
                    }
                    //没找到直接结束
                    else
                    {
                        mcnt_ActionType = Action_None;
                        mcnt_QRLocateCnt = 0;
                        mcnt_QRLocateType = QRLocateType_None;
                        ptimer_QRLocate->stop();
                    }
                }
                else
                {
                    //调试状态，更新步骤状态变量
                    QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
                    mylog<<"wait for next cmd....";
                }
            }
        }
        //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
        if(QRLocateCommand::getInstance().isDebug == true && QRLocateCommand::getInstance().StepCnt == ServoManager::Debug_StepStatus_CmdNext)
        {
            QRLocateCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
            //如果找到相应的定位QR码
            if(mIsFindLocator == true)
            {
                //如果偏差距离小于设定误差，则上报上位机，并运动到下一个定位位置
                if(mLocatorDistance < QRLocateError)
                {
                    //发送信号上报上位机
                    SendLocateResult();
                    mylog<<"locate error"<<mLocatorDistance<<" ok!";
                    if(mcnt_QRLocateType == QRLocateType_X)
                    {
                        if(mcnt_QRLocateCnt < QRLocateCommand::getInstance().totalSub)
                        {
                            mcnt_QRLocateCnt++;
                            //更新动作类型
                            mcnt_ActionType = Action_XLocate;
                            //根据小节尺寸计算新的X位置
                            int subpostion = QRLocateCommand::getInstance().subWidth[mcnt_QRLocateCnt - 1];
                            //下指令
                            SetMoveXY(subpostion,Axis_Origin_INC,Action_MoveXY2StorgeTarget);
                            mLocatorPosition = subpostion;
                            mylog<<"set movex:"<<subpostion;
                        }
                        else if(mcnt_QRLocateCnt == QRLocateCommand::getInstance().totalSub)
                        {
                            mcnt_QRLocateCnt = 1;
                            //更新动作类型
                            mcnt_ActionType = Action_YLocate;
                            mcnt_QRLocateType = QRLocateType_Y;
                            //根据小节尺寸计算下一层的位置
                            int layerpostion = (QRLocateCommand::getInstance().portLayer - mcnt_QRLocateCnt) * AxisY_Layer_INC;
                            //下指令
                            SetMoveXY(QRLocateCommand::getInstance().firstSubPosition,layerpostion,Action_MoveXY2StorgeTarget);
                            mLocatorPosition = layerpostion;
                            mylog<<"set movey:"<<layerpostion;
                        }
                    }
                    else if(mcnt_QRLocateType == QRLocateType_Y)
                    {
                        if(mcnt_QRLocateCnt < QRLocateCommand::getInstance().totalLayer)
                        {
                            mcnt_QRLocateCnt++;
                            //跳过出货口那一层定位，原点已经定位
                            if(mcnt_QRLocateCnt == QRLocateCommand::getInstance().portLayer)
                            {
                                if(mcnt_QRLocateCnt != QRLocateCommand::getInstance().totalLayer)
                                {
                                    mcnt_QRLocateCnt++;
                                }
                                else
                                {
                                    //结束
                                    mcnt_ActionType = Action_None;
                                    mcnt_QRLocateCnt = 0;
                                    mcnt_QRLocateType = QRLocateType_None;
                                    ptimer_QRLocate->stop();
                                }
                            }
                            //更新动作类型
                            mcnt_ActionType = Action_YLocate;
                            //根据小节尺寸计算层的位置
                            int layerpostion = (QRLocateCommand::getInstance().portLayer - mcnt_QRLocateCnt) * AxisY_Layer_INC;
                            //下指令
                            SetMoveXY(QRLocateCommand::getInstance().firstSubPosition,layerpostion,Action_MoveXY2StorgeTarget);
                            mLocatorPosition = layerpostion;
                            mylog<<"set movey:"<<layerpostion;
                        }
                        else if(mcnt_QRLocateCnt == QRLocateCommand::getInstance().totalLayer)
                        {
                            //结束
                            mcnt_ActionType = Action_None;
                            mcnt_QRLocateCnt = 0;
                            mcnt_QRLocateType = QRLocateType_None;
                            ptimer_QRLocate->stop();
                        }
                    }
                }
                //如果误差大于设定值，则加上误差距离后重新运动至定位点
                else
                {
                    mylog<<"error > seterror,mcnt_QRLocateCnt:"<<mcnt_QRLocateCnt<<"realdistance:"<<mLocatorDistance;
                    if(mcnt_QRLocateType == QRLocateType_X)
                    {
                        //根据距离换算成实际的编码器值
                        int inc = (QRLocateCommand::getInstance().subWidth[mcnt_QRLocateCnt-1] + mLocatorDistance)*AxisX_1mm_INC;
                        SetMoveXY(inc,Axis_Origin_INC,Action_MoveXY2StorgeTarget);
                        mcnt_ActionType = Action_XLocate;
                        if(mcnt_QRLocateCnt == 1)
                        {
                            QRLocateCommand::getInstance().firstSubPosition = inc;
                        }
                        mLocatorPosition = inc;
                        mylog<<"set movex:"<<inc;
                    }
                    else
                    {
                        //根据距离换算成实际的编码器值
                        int inc = (QRLocateCommand::getInstance().portLayer - mcnt_QRLocateCnt) * AxisY_Layer_INC - mLocatorDistance*AxisY_1mm_INC;
                        SetMoveXY(QRLocateCommand::getInstance().firstSubPosition,inc,Action_MoveXY2StorgeTarget);
                        mcnt_ActionType = Action_YLocate;
                        mLocatorPosition = inc;
                        mylog<<"set movey:"<<inc;
                    }
                }
            }
            //没找到直接结束
            else
            {
                mcnt_ActionType = Action_None;
                mcnt_QRLocateCnt = 0;
                mcnt_QRLocateType = QRLocateType_None;
                ptimer_QRLocate->stop();
            }
        }
    }
}

CStatus XYMoveNode::run()
{
   CStatus status;
//   CGraph::CGRAPH_ECHO("[%s], enter MyNode2 run function. Sleep for 2 second ... ", this->getName().c_str());
   if(ArvCommand::getInstance().isDebug == true){
       ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
   }
   pServoManager->SetMoveXY(x,y,ServoManager::Action_MoveXY2PortTarget);
   mylog<<"XYMoveNode Set XY to ("<<x<<","<<y<<") start!";
   int cnt = 50;
   while (pipeRunFlag && cnt--) {
       if((pServoManager->pXmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0
               && (pServoManager->pYmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0)
       {
               mylog<<"XYMovePortNode Set XY to ("<<x<<","<<y<<") done!";
               //电机断电
               pServoManager->pXmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
               pServoManager->pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
               //更新电机位置状态
               if(type == XYMOVETYPE::type_Origin){
                   pServoManager->pXmotor->mServoLocation = ServoManager::Location_Origin;
                   pServoManager->pYmotor->mServoLocation = ServoManager::Location_Origin;
                   mylog<<"update servoXY Location_Origin";
               }
               else if(type == XYMOVETYPE::type_Port){
                   pServoManager->pXmotor->mServoLocation = ServoManager::Location_Port;
                   pServoManager->pYmotor->mServoLocation = ServoManager::Location_Port;
                   mylog<<"update servoXY Location_Port";
               }
               else{
                   pServoManager->pXmotor->mServoLocation = ServoManager::Location_NoneOrigin;
                   pServoManager->pYmotor->mServoLocation = ServoManager::Location_NoneOrigin;
                   mylog<<"update servoXY Location_NoneOrigin";
               }

               break;
       }

       CGRAPH_SLEEP_SECOND(1);
   }

   //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
   if(pipeRunFlag && ArvCommand::getInstance().isDebug == true){
       g_DebugMutex.lock();
       //调试状态，更新步骤状态变量
       ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
       mylog<<"Debug wait for next cmd....";
       g_DebugCondition.wait(&g_DebugMutex);
       g_DebugMutex.unlock();
   }
   return status;


}

CStatus ZMoveNode::run()
{
    CStatus status;
 //   CGraph::CGRAPH_ECHO("[%s], enter MyNode2 run function. Sleep for 2 second ... ", this->getName().c_str());
    if(ArvCommand::getInstance().isDebug == true){
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
    }
    //步进电机位置到断电后，位置到信号不会清零，先置为0
    pServoManager->pZmotor->mRo_StatusWord &= 0xFBFF;
    if(type == ZMOVETYPE::type_In){
        pServoManager->SetMoveZ(ZMOVETYPE::type_In);
        mylog<<"ZMoveInNode Set Z to ("<<ServoManager::AxisZ_In<<") start!";
    }
    else{
        pServoManager->SetMoveZ(ZMOVETYPE::type_Out);
        mylog<<"ZMoveInNode Set Z to ("<<ServoManager::AxisZ_Out<<") start!";
    }


    int cnt = 60;
    while (pipeRunFlag && cnt--) {
        if((pServoManager->pZmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0)
        {
            //电机断电
            pServoManager->pZmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            //更新电机位置状态
            if(type == ZMOVETYPE::type_In){
                mylog<<"ZMoveInNode Set Z to ("<<ServoManager::AxisZ_In<<") done!";
                pServoManager->pZmotor->mServoLocation = ServoManager::Location_ZIn;
                mylog<<"update servoZ Location_ZIn";
            }
            else{
                mylog<<"ZMoveInNode Set Z to ("<<ServoManager::AxisZ_Out<<") done!";
                pServoManager->pZmotor->mServoLocation = ServoManager::AxisZ_Out;
                mylog<<"update servoZ AxisZ_Out";
            }

            break;
        }

        CGRAPH_SLEEP_SECOND(1);
    }

    //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
    if(pipeRunFlag && ArvCommand::getInstance().isDebug == true){
        g_DebugMutex.lock();
        //调试状态，更新步骤状态变量
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
        mylog<<"Debug wait for next cmd....";
        g_DebugCondition.wait(&g_DebugMutex);
        g_DebugMutex.unlock();
    }
    return status;

}

CStatus GrabNode::run()
{
    CStatus status;
 //   CGraph::CGRAPH_ECHO("[%s], enter MyNode2 run function. Sleep for 2 second ... ", this->getName().c_str());
    if(ArvCommand::getInstance().isDebug == true){
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
    }
    if(type == GRABTYPE::type_Clampe){
        pServoManager->SetMoveClamp(GRABTYPE::type_Clampe);
        pServoManager->pRmServo->mServoLocation = ServoManager::Location_Clamping;
        mylog<<"GrabNode Set GrabMotor to clampe start!";
    }
    else{
        pServoManager->SetMoveClamp(GRABTYPE::type_Lossen);
        pServoManager->pRmServo->mServoLocation = ServoManager::Location_Loosening;
        mylog<<"GrabNode Set GrabMotor to lossen start!";
    }

    //夹爪直接延时，无完成反馈
    CGRAPH_SLEEP_SECOND(5);

    //更新电机位置状态
    if(type == GRABTYPE::type_Clampe){
        mylog<<"ZMoveInNode Set GrabMotor to clampe done!";
        pServoManager->pZmotor->mServoLocation = ServoManager::Location_Clamped;
        mylog<<"update GrabMotor Location_Clamped";
    }
    else{
        mylog<<"ZMoveInNode Set GrabMotor to lossen done!";
        pServoManager->pZmotor->mServoLocation = ServoManager::Location_Loosened;
        mylog<<"update GrabMotor Location_Loosened";
    }

    //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
    if(pipeRunFlag && ArvCommand::getInstance().isDebug == true){
        g_DebugMutex.lock();
        //调试状态，更新步骤状态变量
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
        mylog<<"Debug wait for next cmd....";
        g_DebugCondition.wait(&g_DebugMutex);
        g_DebugMutex.unlock();
    }
    return status;

}

CStatus YStepNode::run()
{
    CStatus status;
 //   CGraph::CGRAPH_ECHO("[%s], enter MyNode2 run function. Sleep for 2 second ... ", this->getName().c_str());
    if(ArvCommand::getInstance().isDebug == true){
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
    }
    if(type == YSTEPTYPE::type_UP){
        pServoManager->SetYStep(type);
        mylog<<"YStepNode Set Y to UP start!";
    }
    else{
        pServoManager->SetYStep(type);
        mylog<<"YStepNode Set Y to DOWN start!";
    }

    int cnt = 60;
    while (pipeRunFlag && cnt--) {
        if((pServoManager->pYmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0)
        {
            //电机断电
            pServoManager->pYmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            //更新电机位置状态
            if(type == YSTEPTYPE::type_UP){
                mylog<<"YStepNode Set  Y to UP done!";
                pServoManager->pYmotor->mServoLocation = ServoManager::Location_MoveDone;
                mylog<<"update servoY Location_MoveDone";
            }
            else{
                mylog<<"YStepNode Set Y to DOWN done!";
                pServoManager->pYmotor->mServoLocation = ServoManager::Location_MoveDone;
                mylog<<"update servoY Location_MoveDone";
            }

            break;
        }

        CGRAPH_SLEEP_SECOND(1);
    }

    //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
    if(pipeRunFlag && ArvCommand::getInstance().isDebug == true){
        g_DebugMutex.lock();
        //调试状态，更新步骤状态变量
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
        mylog<<"Debug wait for next cmd....";
        g_DebugCondition.wait(&g_DebugMutex);
        g_DebugMutex.unlock();
    }
    return status;
}

CStatus RotateNode::run()
{
    CStatus status;
    if(!enable) return status;
 //   CGraph::CGRAPH_ECHO("[%s], enter MyNode2 run function. Sleep for 2 second ... ", this->getName().c_str());
    if(ArvCommand::getInstance().isDebug == true){
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
    }

    if(type == ROTATETYPE::type_FaceA){
        pServoManager->SetMoveRotate(type);
        mylog<<"YStepNode Set R to FACEA start!";
    }
    else{
        pServoManager->SetMoveRotate(type);
        mylog<<"YStepNode Set R to FACEB start!";
    }

    int cnt = 60;
    while (pipeRunFlag && cnt--) {
        if((pServoManager->pRmotor->mRo_StatusWord & KINCOSTATUSWORD::Status_TargetPositionArrive) !=0)
        {
            //电机断电
            pServoManager->pRmotor->ArriveTargetPosition_PDO(0,KINCOCONTROLWORD::ControlWord_MotorPowerOff);
            //更新电机位置状态
            if(type == ROTATETYPE::type_FaceA){
                mylog<<"YStepNode Set R to FACEA done!";
                pServoManager->pRmotor->mServoLocation = ServoManager::Location_FaceA;
                mylog<<"update servoR Location_FaceA";
            }
            else{
                mylog<<"YStepNode Set R to FACEB done!";
                pServoManager->pRmotor->mServoLocation = ServoManager::Location_FaceB;
                mylog<<"update servoR Location_FaceB";
            }

            break;
        }

        CGRAPH_SLEEP_SECOND(1);
    }

    //如果1.调试状态 2.上面电机目标位置到  3.可以执行下一步
    if(pipeRunFlag && ArvCommand::getInstance().isDebug == true){
        g_DebugMutex.lock();
        //调试状态，更新步骤状态变量
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
        mylog<<"Debug wait for next cmd....";
        g_DebugCondition.wait(&g_DebugMutex);
        g_DebugMutex.unlock();
    }
    return status;
}

CStatus CameraNode::run()
{
    CStatus status;
    if(ArvCommand::getInstance().isDebug == true){
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdDoing);
    }
    QImage img;
    if(pServoManager->pCameraManager->slot_catchImage(img)){
//        QZXing decoder;
//            //mandatory settings
//        decoder.setDecoder( QZXing::DecoderFormat_QR_CODE | QZXing::DecoderFormat_EAN_13 );

//        //optional settings
//        //decoder.setSourceFilterType(QZXing::SourceFilter_ImageNormal | QZXing::SourceFilter_ImageInverted);
//        decoder.setSourceFilterType(QZXing::SourceFilter_ImageNormal);
//        decoder.setTryHarderBehaviour(QZXing::TryHarderBehaviour_ThoroughScanning | QZXing::TryHarderBehaviour_Rotate);

//         //trigger decode
//        QString result = decoder.decodeImage(img);
//        mylog<<"decode qrcode:"<<result;
//        if(result != ArvCommand::getInstance().QRCode){
//            mylog<<"decode qrcode:"<<result<<" != dst qrcode:"<<ArvCommand::getInstance().QRCode;
//        }
    }
    else{
        mylog<<"catch one image failed!";
    }

    if(pipeRunFlag && ArvCommand::getInstance().isDebug == true){
        g_DebugMutex.lock();
        //调试状态，更新步骤状态变量
        ArvCommand::getInstance().SetStep(ServoManager::Debug_StepStatus_CmdOK);
        mylog<<"Debug wait for next cmd....";
        g_DebugCondition.wait(&g_DebugMutex);
        g_DebugMutex.unlock();
    }
    return status;
}
